/*
 * @Author: zwf 240970521@qq.com
 * @Date: 2023-07-18 19:40:00
 * @LastEditors: zwf 240970521@qq.com
 * @LastEditTime: 2023-08-12 20:44:10
 * @FilePath: /esp32_motor/motor/algorithm /common.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */

#include "math_common.h"
#include <math.h>
#include <motor_priv.h>

const _iq24 _2pi     = _IQ(_2PI);
const _iq24 _1_sqrt3 = _IQ(_1_SQRT3);
const _iq24 _sqrt3   = _IQ(_SQRT3);
const _iq24 _2_sqrt3 = _IQ(_2_SQRT3);
const _iq24 _half    = _IQ(0.5);
const _iq24 _3pi_2   = _IQ(_3PI_2);
const _iq24 _pi_2    = _IQ(_PI_2);
const _iq24 _one     = _IQ(1);
const _iq24 _zero    = _IQ(0);
/*最小检测移动距离*/
const _iq24 _min_angle_detect_movement = _IQ(MIN_ANGLE_DETECT_MOVEMENT);

/*电角度*/
_iq24 ElectricalIQAngle(_iq24 angle)
{

#ifdef MOTOR_OBSERVER
    angle %= _2pi;
    if (angle < 0)
    {
        angle = angle + _2pi;
    }
#else

#endif
    return angle;
}

// int16_t ElectricalQAngle(int16_t qAngle)
// {
// #ifdef MOTOR_OBSERVER
//     if (qAngle > 4095)// 调整电角度方位IQ12格式
//         qAngle = qAngle - 4096;
//     else if (qAngle < 0)
//         qAngle = qAngle + 4096;
// #else
//     double elecAngle = (double)(priv->dir) * (double)priv->pp * priv->curAngle.angle - priv->zeroElecAngle;
//     /*速度补偿240M hz下i2c写入到read时长是345us左右, 取一半进行补偿*/
//     if (priv->realSpeed > 50)
//     {
//         elecAngle =
//             priv->realSpeed * (priv->modbusReg.reg.time - 25) * 0.95 * 0.001F * 0.001F * (double)priv->pp + elecAngle;
//     }
// #endif


//     return qAngle & QANGLE_RANGE;
// }


void FastSinCos(_iq24 qAngle, SinCos *out)
{
    out->sinValue = _IQsin(qAngle);
    out->cosValue = _IQcos(qAngle);
}

void ParkTrans(PhasePara *para, SinCos value)
{
    para->q = _IQmpy(para->beta, value.cosValue) - _IQmpy(para->alpha, value.sinValue);
    para->d = _IQmpy(para->alpha, value.cosValue) + _IQmpy(para->beta, value.sinValue);
}

void ParkReversalTrans(PhasePara *para, SinCos value)
{
    para->alpha = _IQmpy(para->d, value.cosValue) - _IQmpy(para->q, value.sinValue);
    para->beta  = _IQmpy(para->d, value.sinValue) + _IQmpy(para->q, value.cosValue);
}

void ClarkTrans(PhasePara *para)
{
    para->alpha = para->a;
    para->beta  = _IQmpy(_1_sqrt3, para->a) + _IQmpy(_2_sqrt3, para->b);
}

void ClarkReversalTrans(PhasePara *para)
{
    para->a = para->alpha;
    para->b = _IQmpy(_IQmpy(_sqrt3, para->beta) - para->alpha, _half);
    para->c = _IQmpy(-para->alpha - _IQmpy(_sqrt3, para->beta), _half);
}
